﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CommonLib
{
    /// <summary>
    /// Vector - A value type which defined a vector in terms of X and Y
    /// </summary> 
    [Serializable]
    public struct Vector
    {
        #region 构造函数

        internal double _x;
        internal double _y;

        /// <summary> 
        /// Constructor which sets the vector's initial values
        /// </summary>
        /// <param name="x"> double - The initial X </param>
        /// <param name="y"> double - THe initial Y </param> 
        public Vector(double x, double y)
        {
            _x = x;
            _y = y;
        }

        /// <summary>
        /// 用模和弧度构造向量(极坐标形式)
        /// </summary>
        /// <param name="length">模</param>
        /// <param name="angle">弧度</param>
        /// <param name="dummy">哑参,为了区分默认构造函数</param>
        public Vector(double length, double angle, int dummy)
        {
            _x = length * Math.Cos(angle);
            _y = length * Math.Sin(angle);
        }

        /// <summary>
        ///     X - double.  Default value is 0. 
        /// </summary>
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }

        /// <summary> 
        ///     Y - double.  Default value is 0.
        /// </summary> 
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }

        /// <summary>
        /// 空向量
        /// </summary>
        public static Vector Empty = new Vector(0, 0);

        #endregion 构造函数

        #region 重载函数

        /// <summary>
        /// Compares two Vector instances for exact equality. 
        /// </summary>
        /// <remarks>
        /// Note that double values can acquire error when operated upon, such that 
        /// an exact comparison between two values which are logically equal may fail.
        /// Furthermore, using this equality operator, Double.NaN is not equal to itself. 
        /// </remarks>
        /// <returns>
        /// bool - true if the two Vector instances are exactly equal, false otherwise
        /// </returns> 
        /// <param name='vector1'>The first Vector to compare</param>
        /// <param name='vector2'>The second Vector to compare</param> 
        public static bool operator ==(Vector vector1, Vector vector2)
        {
            return vector1.X == vector2.X && vector1.Y == vector2.Y;
        }

        /// <summary> 
        /// Compares two Vector instances for exact inequality.
        /// Note that double values can acquire error when operated upon, such that 
        /// an exact comparison between two values which are logically equal may fail. 
        /// Furthermore, using this equality operator, Double.NaN is not equal to itself.
        /// </summary> 
        /// <returns>
        /// bool - true if the two Vector instances are exactly unequal, false otherwise
        /// </returns>
        /// <param name='vector1'>The first Vector to compare</param> 
        /// <param name='vector2'>The second Vector to compare</param>
        public static bool operator !=(Vector vector1, Vector vector2)
        {
            return !(vector1 == vector2);
        }
        /// <summary>
        /// Compares two Vector instances for object equality.  In this equality
        /// Double.NaN is equal to itself, unlike in numeric equality.
        /// Note that double values can acquire error when operated upon, such that 
        /// an exact comparison between two values which
        /// are logically equal may fail. 
        /// </summary> 
        /// <returns>
        /// bool - true if the two Vector instances are exactly equal, false otherwise 
        /// </returns>
        /// <param name='vector1'>The first Vector to compare</param>
        /// <param name='vector2'>The second Vector to compare</param>
        public static bool Equals(Vector vector1, Vector vector2)
        {
            return vector1.X.Equals(vector2.X) && vector1.Y.Equals(vector2.Y);
        }

        /// <summary>
        /// Equals - compares this Vector with the passed in object.  In this equality
        /// Double.NaN is equal to itself, unlike in numeric equality.
        /// Note that double values can acquire error when operated upon, such that 
        /// an exact comparison between two values which
        /// are logically equal may fail. 
        /// </summary> 
        /// <returns>
        /// bool - true if the object is an instance of Vector and if it's equal to "this". 
        /// </returns>
        /// <param name='o'>The object to compare to "this"</param>
        public override bool Equals(object o)
        {
            if ((null == o) || !(o is Vector))
            {
                return false;
            }

            Vector value = (Vector)o;
            return Vector.Equals(this, value);
        }

        /// <summary>
        /// Equals - compares this Vector with the passed in object.  In this equality 
        /// Double.NaN is equal to itself, unlike in numeric equality. 
        /// Note that double values can acquire error when operated upon, such that
        /// an exact comparison between two values which 
        /// are logically equal may fail.
        /// </summary>
        /// <returns>
        /// bool - true if "value" is equal to "this". 
        /// </returns>
        /// <param name='value'>The Vector to compare to "this"</param> 
        public bool Equals(Vector value)
        {
            return Vector.Equals(this, value);
        }

        /// <summary>
        /// Returns the HashCode for this Vector
        /// </summary> 
        /// <returns>
        /// int - the HashCode for this Vector 
        /// </returns> 
        public override int GetHashCode()
        {
            // Perform field-by-field XOR of HashCodes
            return _x.GetHashCode() ^ _y.GetHashCode();
        }

        /// <summary> 
        /// Creates a string representation of this object based on the current culture.
        /// </summary> 
        /// <returns> 
        /// A string representation of this object.
        /// </returns> 
        public override string ToString()
        {
            return String.Format("X={0}, Y={1}", this._x, this._y);
        }

        #endregion 重载函数

        #region 公共函数

        /// <summary> 
        /// Length Property - the length of this Vector 
        /// </summary>
        public double Length
        {
            get
            {
                return Math.Sqrt(_x * _x + _y * _y);
            }
        }

        /// <summary>
        /// LengthSquared Property - the squared length of this Vector 
        /// </summary>
        public double LengthSquared
        {
            get
            {
                return _x * _x + _y * _y;
            }
        }

        /// <summary>
        /// Normalize - Updates this Vector to maintain its direction, but to have a length
        /// of 1.  This is equivalent to dividing this Vector by Length
        /// </summary> 
        public void Normalize()
        {
            // Avoid overflow 
            this /= Math.Max(Math.Abs(_x), Math.Abs(_y));
            this /= Length;
        }

        /// <summary>
        /// 矢量的弧度
        /// </summary>
        public double Angle
        {
            get { return Math.Atan2(this._y, this._x); }
        }

        /// <summary>
        /// CrossProduct - Returns the cross product: vector1.X*vector2.Y - vector1.Y*vector2.X 
        /// </summary>
        /// <returns> 
        /// 判断两个矢量的左右手关系，差乘最方便：v2在v1的逆时针，返回正值;v2在v1的顺时针，返回负值
        /// </returns>
        /// <param name="vector1"> The first Vector </param> 
        /// <param name="vector2"> The second Vector </param>
        public static double CrossProduct(Vector vector1, Vector vector2)
        {
            return vector1._x * vector2._y - vector1._y * vector2._x;
        }

        /// <summary>
        /// 点乘 : vector1.X*vector2.X + vector1.Y*vector2.Y
        /// <remarks>判断两个矢量夹角是否大于90度，点乘判断最便捷</remarks>
        /// </summary>
        /// <param name="vector1"> The first Vector</param>
        /// <param name="vector2">The second Vector</param>
        /// <returns></returns>
        public static double DotProduct(Vector vector1, Vector vector2)
        {
            return vector1._x * vector2._x + vector1._y * vector2._y;
        }

        /// <summary> 
        /// AngleBetween - the angle between 2 vectors[-PI,PI]
        /// </summary> 
        /// <returns>
        /// Returns the the angle between vector1 and vector2
        /// 经验证:返回值逆时针为正，顺时针为负
        /// </returns>
        /// <param name="vector1"> The first Vector </param> 
        /// <param name="vector2"> The second Vector </param>
        public static double AngleBetween(Vector vector1, Vector vector2)
        {
            double sin = vector1._x * vector2._y - vector2._x * vector1._y;
            double cos = vector1._x * vector2._x + vector1._y * vector2._y;

            return Math.Atan2(sin, cos);
        }

        /// <summary>
        /// 计算两个向量之间的旋转角[0, PI*2)
        /// </summary>
        /// <remarks>
        /// 首先明确几个数学概念：
        /// 1. 极轴沿逆时针转动的方向是正方向
        /// 2. 两个向量之间的夹角theta， 是指(A^B)/(|A|*|B|) = cos(theta)，0&lt;=theta&gt;=180 度， 而且没有方向之分
        /// 3. 两个向量的旋转角，是指从向量p1开始，逆时针旋转，转到向量p2时，所转过的角度， 范围是 0 ~ 360度
        /// 计算向量p1到p2的旋转角，算法如下：
        /// 首先通过点乘和arccosine的得到两个向量之间的夹角
        /// 然后判断通过差乘来判断两个向量之间的位置关系
        /// 如果p2在p1的顺时针方向, 返回arccose的角度值, 范围0 ~ 180.0(根据右手定理,可以构成正的面积)
        /// 否则返回 360.0 - arecose的值, 返回180到360(根据右手定理,面积为负)
        /// </remarks>
        /// <param name="v1">The second Vector</param>
        /// <param name="v2">The second Vector</param>
        /// <param name="clockwise">顺时针？</param>
        /// <returns>两向量的旋转角</returns>
        public static double AngleRotated(Vector v1, Vector v2, bool clockwise)
        {
            // normalize
            v1.Normalize();
            v2.Normalize();
            // dot product
            double dot = Vector.DotProduct(v1, v2);
            if (dot > 1)
                dot = 1;
            else if (dot < -1)
                dot = -1;
            double angle = Math.Acos(dot);
            //cross product
            //double cross = x1 * y2 - x2 * y1;
            double cross = -Vector.CrossProduct(v1, v2);
            // vector p2 is clockwise from vector p1 
            // with respect to the origin (0.0)
            if (clockwise)
                angle = cross >= 0 ? angle : Math.PI * 2 - angle;
            else
                angle = cross >= 0 ? Math.PI * 2 - angle : angle;
            return angle;
        }

        /// <summary>
        /// 计算两个向量的垂直线的交点向量
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>两向量的垂直线的交点向量</returns>
        //public static Vector VerticalIntersct(Vector v1, Vector v2)
        //{
        //    Line l1 = new Line(v1.Angle + Math.PI * .5, (Point)v1);
        //    Line l2 = new Line(v2.Angle + Math.PI * .5, (Point)v2);
        //    Point point;
        //    if (!Line.Intersect(l1, l2, out point))
        //        throw new Exception("向量不能平行");
        //    return new Vector(point._x, point._y);
        //}

        /// <summary>
        /// 矢量旋转
        /// </summary>
        /// <param name="angle">逆时针旋转角度为正</param>
        public void Rotate(double angle)
        {
            double new_angle = Angle + angle;
            double length = Length;
            _x = length * Math.Cos(new_angle);
            _y = length * Math.Sin(new_angle);
        }

        /// <summary>
        /// 矢量的缩放(纵横比允许不一致)
        /// </summary>
        /// <param name="scale_x"></param>
        /// <param name="scale_y"></param>
        public void Scale(double scale_x, double scale_y)
        {
            _x *= scale_x;
            _y *= scale_y;
        }

        #endregion 公共函数

        #region 运算重载

        /// <summary>
        /// Operator -Vector (unary negation) 
        /// </summary>
        public static Vector operator -(Vector vector)
        {
            return new Vector(-vector._x, -vector._y);
        }

        /// <summary> 
        /// Negates the values of X and Y on this Vector
        /// </summary> 
        public void Negate()
        {
            _x = -_x;
            _y = -_y;
        }

        /// <summary> 
        /// Operator Vector + Vector
        /// </summary> 
        public static Vector operator +(Vector vector1, Vector vector2)
        {
            return new Vector(vector1._x + vector2._x,
                              vector1._y + vector2._y);
        }

        /// <summary> 
        /// Add: Vector + Vector
        /// </summary> 
        public static Vector Add(Vector vector1, Vector vector2)
        {
            return new Vector(vector1._x + vector2._x,
                              vector1._y + vector2._y);
        }

        /// <summary> 
        /// Operator Vector - Vector
        /// </summary> 
        public static Vector operator -(Vector vector1, Vector vector2)
        {
            return new Vector(vector1._x - vector2._x,
                              vector1._y - vector2._y);
        }

        /// <summary> 
        /// Subtract: Vector - Vector
        /// </summary> 
        public static Vector Subtract(Vector vector1, Vector vector2)
        {
            return new Vector(vector1._x - vector2._x,
                              vector1._y - vector2._y);
        }

        /// <summary> 
        /// Operator Vector + Point
        /// </summary> 
        //public static Point operator +(Vector vector, Point point)
        //{
        //    return new Point(point._x + vector._x, point._y + vector._y);
        //}

        /// <summary> 
        /// Add: Vector + Point 
        /// </summary>
        //public static Point Add(Vector vector, Point point)
        //{
        //    return new Point(point._x + vector._x, point._y + vector._y);
        //}

        /// <summary>
        /// Operator Vector * double 
        /// </summary> 
        public static Vector operator *(Vector vector, double scalar)
        {
            return new Vector(vector._x * scalar,
                              vector._y * scalar);
        }

        /// <summary>
        /// Multiply: Vector * double 
        /// </summary> 
        public static Vector Multiply(Vector vector, double scalar)
        {
            return new Vector(vector._x * scalar,
                              vector._y * scalar);
        }

        /// <summary>
        /// Operator double * Vector 
        /// </summary> 
        public static Vector operator *(double scalar, Vector vector)
        {
            return new Vector(vector._x * scalar,
                              vector._y * scalar);
        }

        /// <summary>
        /// Multiply: double * Vector 
        /// </summary> 
        public static Vector Multiply(double scalar, Vector vector)
        {
            return new Vector(vector._x * scalar,
                              vector._y * scalar);
        }

        /// <summary>
        /// Operator Vector / double 
        /// </summary> 
        public static Vector operator /(Vector vector, double scalar)
        {
            return vector * (1.0 / scalar);
        }

        /// <summary> 
        /// Multiply: Vector / double
        /// </summary> 
        public static Vector Divide(Vector vector, double scalar)
        {
            return vector * (1.0 / scalar);
        }

        // <summary>
        // Operator Vector * Matrix 
        // </summary>
        //public static Vector operator * (Vector vector, Matrix matrix) 
        //{ 
        //    return matrix.Transform(vector);
        //} 

        // <summary>
        // Multiply: Vector * Matrix
        // </summary> 
        //public static Vector Multiply(Vector vector, Matrix matrix)
        //{ 
        //    return matrix.Transform(vector); 
        //}

        /// <summary>
        /// Operator Vector * Vector, interpreted as their dot product
        /// </summary>
        public static double operator *(Vector vector1, Vector vector2)
        {
            return vector1._x * vector2._x + vector1._y * vector2._y;
        }

        /// <summary> 
        /// Multiply - Returns the dot product: vector1.X*vector2.X + vector1.Y*vector2.Y
        /// </summary>
        /// <returns>
        /// Returns the dot product: vector1.X*vector2.X + vector1.Y*vector2.Y 
        /// </returns>
        /// <param name="vector1"> The first Vector </param> 
        /// <param name="vector2"> The second Vector </param> 
        public static double Multiply(Vector vector1, Vector vector2)
        {
            return vector1._x * vector2._x + vector1._y * vector2._y;
        }

        /// <summary> 
        /// Determinant - Returns the determinant det(vector1, vector2)
        /// </summary> 
        /// <returns> 
        /// Returns the determinant: vector1.X*vector2.Y - vector1.Y*vector2.X
        /// </returns> 
        /// <param name="vector1"> The first Vector </param>
        /// <param name="vector2"> The second Vector </param>
        public static double Determinant(Vector vector1, Vector vector2)
        {
            return vector1._x * vector2._y - vector1._y * vector2._x;
        }

        /// <summary>
        /// Explicit conversion to Size.  Note that since Size cannot contain negative values, 
        /// the resulting size will contains the absolute values of X and Y
        /// </summary>
        /// <returns>
        /// Size - A Size equal to this Vector 
        /// </returns>
        /// <param name="vector"> Vector - the Vector to convert to a Size </param> 
        //public static explicit operator Size(Vector vector)
        //{
        //    return new Size(Math.Abs(vector._x), Math.Abs(vector._y));
        //}

        /// <summary>
        /// Explicit conversion to Point 
        /// </summary>
        /// <returns> 
        /// Point - A Point equal to this Vector 
        /// </returns>
        /// <param name="vector"> Vector - the Vector to convert to a Point </param> 
        //public static explicit operator Point(Vector vector)
        //{
        //    return new Point(vector._x, vector._y);
        //}

        /// <summary>
        /// minimum mean square error (MMSE)
        /// <remarks>
        /// Base on 钟靖的算法:
        /// 给定一组矢量，计算出一个矢量，让矢量差的和最小,
        /// 目前用于计算图形最佳偏移量，让各点偏移量之和最小
        /// </remarks>
        /// </summary>
        /// <param name="data"></param>
        /// <returns></returns>
        public static Vector MMSE(IEnumerable<Vector> data)
        {
            // 1、变极坐标形式
            List<double> lens = new List<double>();
            List<double> angles = new List<double>();
            foreach (Vector v in data)
            {
                lens.Add(v.Length);
                angles.Add(v.Angle);
            }

            if (lens.Count == 0)
                throw new ArgumentException("collection empty", "data");
            else if (lens.Count == 1)
                return new Vector(lens[0], angles[0], 0);

            // 2、
            double t1 = 0, t2 = 0, t3 = 0, t4 = 0, t5 = 0;
            for (int i = 0; i < lens.Count; i++)
            {
                t1 += Math.Cos(angles[i]) * Math.Cos(angles[i]);
                t2 += Math.Sin(angles[i]) * Math.Sin(angles[i]);
                t3 += Math.Cos(angles[i]) * lens[i];
                t4 += Math.Sin(angles[i]) * lens[i];
                t5 += Math.Cos(angles[i]) * Math.Sin(angles[i]);
            }

            double temp = t1 * t2 - t5 * t5; //要考虑分母为0的时候，如全部是一样的向量的时候
            if (temp == 0)
                return new Vector(lens[0], angles[0], 0);
            else
                return new Vector((t2 * t3 - t4 * t5) / temp, (t3 * t5 - t1 * t4) / -temp);

            /*double distance = 0;//距离平方和
            for (int i = 0; i < xjihe.Count; i++)
            {
                distance = distance + ((x_result * xjihe[i] + y_result * yjihe[i]) / luo[i] - luo[i]) * ((x_result * xjihe[i] + y_result * yjihe[i]) / luo[i] - luo[i]);
            }*/
        }

        /// <summary>
        /// 从向量矩阵获得最近的垂直向量的X
        /// <remarks>常用于获得临近块的偏移值</remarks>
        /// </summary>
        /// <param name="data"></param>
        /// <param name="row"></param>
        /// <param name="col"></param>
        /// <param name="new_row"></param>
        /// <param name="new_col"></param>
        /// <param name="only_same_col"></param>
        /// <returns></returns>
        public static bool GetNearX(Vector[,] data, int row, int col, out int new_row, out int new_col, bool only_same_col)
        {
            Debug.Assert(data != null);
            Debug.Assert(row >= 0 && row < data.GetLength(0));
            Debug.Assert(col >= 0 && col < data.GetLength(1));
            new_row = new_col = 0;
            for (int c = 0; (col - c >= 0) || (col + c < data.GetLength(1)); c++)
            {
                for (int r = 0; (row - r >= 0) || (row + r < data.GetLength(0)); r++)
                {
                    if (r + c == 0) continue;
                    new_row = row - r; new_col = col - c;
                    if (new_row >= 0 && new_col >= 0 &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._x))
                        return true;
                    new_row = row - r; new_col = col + c;
                    if (new_row >= 0 && new_col < data.GetLength(1) &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._x))
                        return true;
                    new_row = row + r; new_col = col - c;
                    if (new_row < data.GetLength(0) && new_col >= 0 &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._x))
                        return true;
                    new_row = row + r; new_col = col + c;
                    if (new_row < data.GetLength(0) && new_col < data.GetLength(1) &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._x))
                        return true;
                }
                if (only_same_col) break;
            }
            return false;
        }

        /// <summary>
        /// 从向量矩阵获得最近的垂直向量的X
        /// <remarks>常用于获得临近块的偏移值</remarks>
        /// </summary>
        /// <param name="data"></param>
        /// <param name="row"></param>
        /// <param name="col"></param>
        /// <param name="new_row"></param>
        /// <param name="new_col"></param>
        /// <param name="only_same_row"></param>
        /// <returns></returns>
        public static bool GetNearY(Vector[,] data, int row, int col, out int new_row, out int new_col, bool only_same_row)
        {
            Debug.Assert(data != null);
            Debug.Assert(row >= 0 && row < data.GetLength(0));
            Debug.Assert(col >= 0 && col < data.GetLength(1));
            new_row = new_col = 0;
            for (int r = 0; (row - r >= 0) || (row + r < data.GetLength(0)); r++)
            {
                for (int c = 0; (col - c >= 0) || (col + c < data.GetLength(1)); c++)
                {
                    if (r + c == 0) continue;
                    new_row = row - r; new_col = col - c;
                    if (new_row >= 0 && new_col >= 0 &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._y))
                        return true;
                    new_row = row - r; new_col = col + c;
                    if (new_row >= 0 && new_col < data.GetLength(1) &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._y))
                        return true;
                    new_row = row + r; new_col = col - c;
                    if (new_row < data.GetLength(0) && new_col >= 0 &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._y))
                        return true;
                    new_row = row + r; new_col = col + c;
                    if (new_row < data.GetLength(0) && new_col < data.GetLength(1) &&
                        data[new_row, new_col] != Vector.Empty && !double.IsNaN(data[new_row, new_col]._y))
                        return true;
                }
                if (only_same_row) break;
            }
            return false;
        }

        #endregion 运算重载
    }
}
